# %% 导入库
'''
使用matplotlib创建图表，并显示在tk窗口
'''
import matplotlib.pyplot as plt
from matplotlib.pylab import mpl
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2Tk
import tkinter as tk
import numpy as np
import time,sys
import threading
import serial
import queue

# 解决中文显示问题

mpl.rcParams['font.sans-serif'] = ['SimHei']  # 中文显示
mpl.rcParams['axes.unicode_minus'] = False  # 负号显示

# 常量定义
BAUD_RATE = 115200
MATH_PI=3.1415926535897932

# %% Atcar类定义
# AtCar类定义
class AtCar:
    """
    类AtCar的定义
    Init函数参数:
        串口号: serial_number
        通信波特率: baud_rate
    """
    def __init__(self, serial_number, baud_rate=115200):
        self.serial_number = serial_number  # 串口号
        self.baud_rate = baud_rate  # 通信波特率
        self.ser_car = serial.Serial(serial_number, baud_rate, timeout=1)  # 打开串口，并设置读取超时时间为1秒
        # 小车参数
        self.wheel_radius = 0.04  # 轮子半径
        self.wheel_y_distance = 0.195  # 轮子左右间距
        self.wheel_x_distance = 0.13  # 轮子前后间距
        # 电机参数
        # 电机编号映射表
        # self.motor_dict = {'left_front': 7, 'right_front': 6, 'left_rear': 9, 'right_rear': 8}
        self.motor_nums = [7, 6, 9, 8]
        # 各个电机每转一圈脉冲数，单位为脉冲数，分别对应 左前、右前、左后、右后 电机
        self.motor_pulse_per_round = [749, 376, 377, 400]
        self.motor_pwms = [1500, 1500, 1500, 1500]  # 电机当前PWM值
        self.motors_speed = [0.0, 0.0, 0.0, 0.0]  # 电机当前速度
        self.motor_speed_pulses = [0, 0, 0, 0]  # 电机当前速度对应的脉冲数
        self.motor_direction = [0, 0, 0, 0]  # 电机当前方向
        self.running = 1  # 线程运行标志, 1为运行，0为停止,-1为强制停止线程

        # 创建线程
        # 获取的是脉冲数还是速度，可自行设置
        self.thread_car = threading.Thread(target=self.get_wheels_speed_and_set_motor_speed_PULES)
        # 如果串口已经连接，则启动线程
        self.init_serial_connection()
        
    def set_car_parameters(self, wheel_radius, wheel_y_distance, wheel_x_distance):
        """
        设置小车参数
        :param wheel_radius: 轮子半径
        :param wheel_y_distance: 轮子左右间距
        :param wheel_x_distance: 轮子前后间距
        """
        self.wheel_radius = wheel_radius
        self.wheel_y_distance = wheel_y_distance
        self.wheel_x_distance = wheel_x_distance
        
    def get_car_parameters(self):
        """
        获取小车参数
        :return: 轮子半径、轮子左右间距、轮子前后间距
        """
        return [self.wheel_radius, self.wheel_y_distance, self.wheel_x_distance]

    def set_motor_parameters(self, motor_pulse_per_round, motor_pwms):
        """
        设置电机参数
        :param motor_pulse_per_round: 各个电机每转一圈脉冲数，单位为脉冲数，分别对应 左前、右前、左后、右后 电机
        :param motor_pwms: 各个电机的初始PWM值
        """
        self.motor_pulse_per_round = motor_pulse_per_round
        self.motor_pwms = motor_pwms

    def init_serial_connection(self):
        """
        初始化串口连接，并启动线程
        """
        try:
            while True:
                ret = self.ser_car.read_all().decode()
                if '[FinishInit]' in ret:
                    break
            print("[INFO] 串口连接初始化完成")
            time.sleep(0.1)
            # print("串口已连接，启动线程")
            self.thread_car.start()
        except serial.SerialException as e:
            print(f"[ERROR] 串口连接初始化失败: {e}")
            self.ser_car.close()
        except Exception as e:
            print(f"[ERROR] 串口连接初始化发生异常: {e}")

    def __del__(self):
        """
        析构函数，在对象被删除时调用
        """
        self.close()

    def close(self):
        """
        结束线程并关闭串口
        """
        self.running = -1
        time.sleep(0.1)  # 等待线程结束
        try:
            if self.thread_car.is_alive():
                self.thread_car.join(timeout=0.5)  # 等待线程结束，最多等待0.5秒
            self.ser_car.close()  # 关闭串口
        except Exception as e:
            print(f"[ERROR] 关闭串口时发生异常: {e}")

    def get_motors_raw_speed(self):
        """
        获取轮子速度, PWM值
        """
        return self.motor_pwms
    
    def get_motor_raw_speed(self, motor_id):
        """
        获取指定电机速度, PWM值
        """
        return self.motor_pwms[motor_id]

    def set_motors_pwm(self, motor_pwms):
        """
        设置电机PWM值, 单位为脉冲数(500-2500)
        """
        # 剔除超出范围的PWM值
        for i in range(4):
            if motor_pwms[i] < 500 or motor_pwms[i] > 2500:
                motor_pwms[i] = int(self.motor_pwms[i])
            else:
                motor_pwms[i] = int(motor_pwms[i])
            self.set_motor_speed(i,motor_pwms[i])

    def get_wheels_speed_and_set_motor_speed(self):
        """
        获取轮子速度，并设置电机速度，类私有函数
        """
        running_time_set = 0.1

        while self.running == 1:
            cur_time = time.time()  # 获取当前时间，用于计算运行时间
            spd_set_cmd = "{{#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!}}".format(
                self.motor_nums[0], self.motor_pwms[0], self.motor_nums[1], self.motor_pwms[1], self.motor_nums[2], self.motor_pwms[2], self.motor_nums[3], self.motor_pwms[3])  # 生成设置电机速度的命令
            try:
                self.ser_car.write(spd_set_cmd.encode())  # 发送设置电机速度的命令
            except serial.SerialException as e:
                print(f"[ERROR] 发送设置电机速度命令时发生错误: {e}")
                self.running = -1
                break

            time.sleep(0.015)  # 等待15ms，以便接收到返回值
            ret = self.ser_car.read_all().decode()  # 接收返回值

            # 对于每一个电机，获取脉冲数
            for num in range(4):
                motor_number = self.motor_nums[num]  # 获取电机编号
                get_speed_cmd = "#{:03d}PSPD!".format(motor_number)  # 生成获取电机速度的命令
                try:
                    self.ser_car.write(get_speed_cmd.encode())  # 发送获取电机速度的命令
                except serial.SerialException as e:
                    print(f"[ERROR] 发送获取电机{motor_number}速度命令时发生错误: {e}")
                    self.running = -1
                    break

                time.sleep(0.015)  # 等待15ms，以便接收到返回值
                ret = self.ser_car.read_all().decode()  # 接收返回值
                try:
                    ret_int = int(ret.split('=')[1].split('/')[0])  # 提取脉冲数
                except (IndexError, ValueError) as e:
                    print(f"[ERROR] 解析电机{motor_number}速度时发生错误: {e}")
                    self.running = -1
                    break

                running_time = time.time() - cur_time  # 计算运行时间
                try:
                    self.motors_speed[num] = (ret_int / self.motor_pulse_per_round[num]) * 2 * MATH_PI * self.wheel_radius / running_time  # 计算电机速度，单位为m/s
                    # 设置正负
                    self.motors_speed[num] *= self.motor_direction[num]
                except ZeroDivisionError as e:
                    print(f"[ERROR] 计算电机{motor_number}速度时发生错误: {e}")
                    self.running = -1
                    break

            running_time = time.time() - cur_time  # 计算运行时间
            if running_time < running_time_set:  # 如果运行时间小于设置时间，则sleep一会儿
                # print("[NO]", running_time)
                time.sleep(running_time_set - running_time)
            else:
                # print("[OVER]", running_time)
                pass

        if self.running == -1:
            print("[INFO] 线程被强制停止")

    def get_wheels_speed_and_set_motor_speed_PULES(self):
        """
        获取轮子速度脉冲值，并设置，类私有函数
        """
        running_time_set = 0.1

        while self.running == 1:
            cur_time = time.time()  # 获取当前时间，用于计算运行时间
            spd_set_cmd = "{{#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!}}".format(
                self.motor_nums[0], self.motor_pwms[0], self.motor_nums[1], self.motor_pwms[1], self.motor_nums[2], self.motor_pwms[2], self.motor_nums[3], self.motor_pwms[3])  # 生成设置电机速度的命令
            try:
                self.ser_car.write(spd_set_cmd.encode())  # 发送设置电机速度的命令
            except serial.SerialException as e:
                print(f"[ERROR] 发送设置电机速度命令时发生错误: {e}")
                self.running = -1
                break

            time.sleep(0.015)  # 等待15ms，以便接收到返回值
            ret = self.ser_car.read_all().decode()  # 接收返回值

            # 对于每一个电机，获取脉冲数
            for num in range(4):
                motor_number = self.motor_nums[num]  # 获取电机编号
                get_speed_cmd = "#{:03d}PSPD!".format(motor_number)  # 生成获取电机速度的命令
                try:
                    self.ser_car.write(get_speed_cmd.encode())  # 发送获取电机速度的命令
                except serial.SerialException as e:
                    print(f"[ERROR] 发送获取电机{motor_number}速度命令时发生错误: {e}")
                    self.running = -1
                    break

                time.sleep(0.015)  # 等待15ms，以便接收到返回值
                ret = self.ser_car.read_all().decode()  # 接收返回值
                try:
                    ret_int = int(ret.split('=')[1].split('/')[0])  # 提取脉冲数
                except (IndexError, ValueError) as e:
                    print(f"[ERROR] 解析电机{motor_number}速度时发生错误: {e}")
                    self.running = -1
                    break
                # self.motor_speed_pulses[num] = ret_int*self.motor_direction[num]  # 保存脉冲数,顺便设置正负
                self.motor_speed_pulses[num] += ret_int

            running_time = time.time() - cur_time  # 计算运行时间
            if running_time < running_time_set:  # 如果运行时间小于设置时间，则sleep一会儿
                # print("[NO]", running_time)
                time.sleep(running_time_set - running_time)
            else:
                # print("[OVER]", running_time)
                pass

        if self.running == -1:
            print("[INFO] 线程被强制停止")

    def set_motor_speed(self, motor_id, speed_pwm):
        """
        设置电机速度, 单位为PWM值(500-2500)
        """
        if motor_id < 0 or motor_id > 3:
            print(f"[ERROR] 电机编号{motor_id}超出范围")
            return
        if speed_pwm < 500 or speed_pwm > 2500:
            print(f"[ERROR] ID{motor_id}电机速度{speed_pwm}超出范围")
            return
        if speed_pwm == 1500:
            self.motor_direction[motor_id] = 0
        elif speed_pwm > 1500:
            self.motor_direction[motor_id] = 1
        elif speed_pwm < 1500:
            self.motor_direction[motor_id] = -1
        self.motor_pwms[motor_id] = speed_pwm

    def get_motor_speed(self, motor_id):
        """
        获取指定电机速度, 单位为m/s
        """
        return self.motors_speed[motor_id]
    
    def get_motors_speed(self):
        """
        获取四个电机速度, 单位为m/s
        """
        return self.motors_speed
    
    def get_motor_speed_pulses(self, motor_id):
        """
        获取指定电机速度, 单位为脉冲数
        """
        return self.motor_speed_pulses[motor_id]
    
    def get_motors_speed_pulses(self):
        """
        获取四个电机速度, 单位为脉冲数
        """
        return self.motor_speed_pulses

# %% 基本变量定义
atcar=AtCar('COM4')

global win
global tempGraphLabel, tempData, runFlag
runFlag = True
tempData = [[],[],[],[],[],[],[],[]]

motor_current_speed=[0,0,0,0]
motor_direction=[0,0,0,0]

DEBUG_MODE = True

# %% 窗口生成
'''
图表类，定义时参数root为父控件
'''
class tempGraph():
    def __init__(self, root):
        self.root = root  # 主窗体
        self.canvas = tk.Canvas()  # 创建一块显示图形的画布
        self.figure = self.create_matplotlib()  # 返回matplotlib所画图形的figure对象
        self.showGraphIn(self.figure)  # 将figure显示在tkinter窗体上面

    '''生成fig'''
    def create_matplotlib(self):
        # 创建绘图对象f
        f = plt.figure(num=2, figsize=(16, 8), dpi=100, edgecolor='green', frameon=True,)
        # 创建一副子图
        self.fig11 = plt.subplot(1, 1, 1)
        self.fig11.set_xlim(0, 5000)  # 设置x轴范围
        self.fig11.set_ylim(0, 1000)  # 设置y轴范围
        # 当前速度曲线
        self.line11, = self.fig11.plot([], [],color='red', label='电机1')
        self.line12, = self.fig11.plot([], [],color='blue', label='电机2')
        self.line13, = self.fig11.plot([], [],color='green', label='电机3')
        self.line14, = self.fig11.plot([], [],color='black', label='电机4')
        # 当前目标速度曲线
        self.line21, = self.fig11.plot([], [],color='red', linestyle='--', label='电机1目标')
        self.line22, = self.fig11.plot([], [],color='blue', linestyle='--', label='电机2目标')
        self.line23, = self.fig11.plot([], [],color='green', linestyle='--', label='电机3目标')
        self.line24, = self.fig11.plot([], [],color='black', linestyle='--', label='电机4目标')

        def setLabel(fig, title, titleColor="red"):
            fig.set_title(title+"速度曲线", color=titleColor)  # 设置标题
            fig.set_xlabel('时间[s]')  # 设置x轴标签
            fig.set_ylabel("脉冲数") # 设置y轴标签
        setLabel(self.fig11, "设备1")
        # fig1.set_yticks([-1, -1 / 2, 0, 1 / 2, 1])  # 设置坐标轴刻度
        f.tight_layout() # 自动紧凑布局
        return f

    '''把fig显示到tkinter'''
    def showGraphIn(self, figure):
        # 把绘制的图形显示到tkinter窗口上
        self.canvas = FigureCanvasTkAgg(figure, self.root)
        self.canvas.draw()  # 以前的版本使用show()方法，matplotlib 2.2之后不再推荐show（）用draw代替，但是用show不会报错，会显示警告
        self.canvas.get_tk_widget().pack(side=tk.TOP) #, fill=tk.BOTH, expand=1

        # 把matplotlib绘制图形的导航工具栏显示到tkinter窗口上
        toolbar = NavigationToolbar2Tk(self.canvas,
                                        self.root)  # matplotlib 2.2版本之后推荐使用NavigationToolbar2Tk，若使用NavigationToolbar2TkAgg会警告
        toolbar.update()
        self.canvas._tkcanvas.pack(side=tk.TOP, fill=tk.BOTH, expand=1)

    '''更新fig'''
    def updateMeltGraph(self, meltData):
        x = [i for i in range(len(meltData[0]))]
        self.line11.set_xdata(x) # x轴也必须更新
        self.line11.set_ydata(meltData[0])  # 更新y轴数据
        self.line12.set_xdata(x)
        self.line12.set_ydata(meltData[1])
        self.line13.set_xdata(x)
        self.line13.set_ydata(meltData[2])
        self.line14.set_xdata(x)
        self.line14.set_ydata(meltData[3])
        self.line21.set_xdata(x)
        self.line21.set_ydata(meltData[4])
        self.line22.set_xdata(x)
        self.line22.set_ydata(meltData[5])
        self.line23.set_xdata(x)
        self.line23.set_ydata(meltData[6])
        self.line24.set_xdata(x)
        self.line24.set_ydata(meltData[7])
        #  更新x数据，但未更新绘图范围。当我把新数据放在绘图上时，它完全超出了范围。解决办法是增加：
        self.fig11.relim()
        self.fig11.autoscale_view()
        plt.draw()
        # self.canvas.draw_idle()
'''
更新数据，在次线程中运行
'''
def updataData():
    global tempData,runFlag
    while runFlag:
        # tempData.append(5)
        # 更新电机速度=====================
        for i in range(4):
            motor_current_speed[i] = atcar.get_motor_speed_pulses(i)
            # motor_direction[i] = atcar.motor_direction[i]
            # motor_current_speed[i] *= motor_direction[i]
            tempData[i].append(motor_current_speed[i])
        for i in range(4):
            tempData[i+4].append(motor_target_speed[i])
        # tempData[0].append(atcar.get_motor_speed(0))
        print(motor_current_speed)
        time.sleep(0.05)
'''
更新窗口
'''
def updateWindow():
    global win
    global tempGraphLabel, tempData, runFlag
    if runFlag:
        tempGraphLabel.updateMeltGraph(tempData)
    win.after(100, updateWindow)  # 1000ms更新画布
'''
关闭窗口触发函数，关闭S7连接，置位flag
'''
def closeWindow():
    global runFlag
    runFlag = False
    sys.exit()

def AddUI_ComPWM(root):
    def update_motor_pwm(event):
        global motor_pwm
        # 获取滑块的值并更新motor_pwm
        motor_pwm = [scale1.get(), scale2.get(), scale3.get(), scale4.get()]
        # print(f"Motor PWM Values: {motor_pwm}")
        atcar.set_motors_pwm(motor_pwm)
        # print(f"Motor PWM Values: {motor_pwm}")

    # 创建四个滑块
    scale1 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 1 PWM")
    scale1.set(1500)  # 设置初始值
    scale1.pack(fill=tk.X, padx=20, pady=5)

    scale2 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 2 PWM")
    scale2.set(1500)  # 设置初始值
    scale2.pack(fill=tk.X, padx=20, pady=5)

    scale3 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 3 PWM")
    scale3.set(1500)  # 设置初始值
    scale3.pack(fill=tk.X, padx=20, pady=5)

    scale4 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 4 PWM")
    scale4.set(1500)  # 设置初始值
    scale4.pack(fill=tk.X, padx=20, pady=5)


    # 绑定事件处理函数:当滑块的值发生变化时，调用update_motor_pwm函数
    scale1.bind("<B1-Motion>", update_motor_pwm)
    scale2.bind("<B1-Motion>", update_motor_pwm)
    scale3.bind("<B1-Motion>", update_motor_pwm)
    scale4.bind("<B1-Motion>", update_motor_pwm)

def AddUI_ComSPD(root):
    # scale Argument:
    # from_ : 最小值
    # to : 最大值
    # resolution : 精度
    scale_resolution = 1
    scale_from = -200
    scale_to = 200
    def update_motor_spd(event):
        global motor_target_speed
        # 获取滑块的值并更新motor_pwm
        motor_target_speed = [scale1.get(), scale2.get(), scale3.get(), scale4.get()]
        # print(f"Motor PWM Values: {motor_pwm}")
        # atcar.set_motors_pwm(motor_pwm)
        # print(f"Motor PWM Values: {motor_pwm}")

    # 创建四个滑块
    scale1 = tk.Scale(root, from_=scale_from, to=scale_to, resolution=scale_resolution, orient=tk.HORIZONTAL, label="Motor 1 SPD")
    scale1.set(0)  # 设置初始值
    scale1.pack(fill=tk.X, padx=20, pady=5)
    scale2 = tk.Scale(root, from_=scale_from, to=scale_to, resolution=scale_resolution, orient=tk.HORIZONTAL, label="Motor 2 SPD")
    scale2.set(0)  # 设置初始值
    scale2.pack(fill=tk.X, padx=20, pady=5)
    scale3 = tk.Scale(root, from_=scale_from, to=scale_to, resolution=scale_resolution, orient=tk.HORIZONTAL, label="Motor 3 SPD")
    scale3.set(0)  # 设置初始值
    scale3.pack(fill=tk.X, padx=20, pady=5)
    scale4 = tk.Scale(root, from_=scale_from, to=scale_to, resolution=scale_resolution, orient=tk.HORIZONTAL, label="Motor 4 SPD")
    scale4.set(0)  # 设置初始值
    scale4.pack(fill=tk.X, padx=20, pady=5)
    
    # 创建一个标签，并提供一个函数用于外部调用，以更新标签内容
    global motor_current_speed_label
    motor_current_speed_label = tk.Label(root, text="Motor Current SPD:")
    motor_current_speed_label.pack(fill=tk.X, padx=20, pady=5)

    # 绑定事件处理函数:当滑块的值发生变化时，调用update_motor_pwm函数
    scale1.bind("<B1-Motion>", update_motor_spd)
    scale2.bind("<B1-Motion>", update_motor_spd)
    scale3.bind("<B1-Motion>", update_motor_spd)
    scale4.bind("<B1-Motion>", update_motor_spd)

# 更改标签内容
def update_motor_current_speed_label(string):
    global motor_current_speed_label
    motor_current_speed_label.config(text=string)

# 创建控制子窗口
def create_new_window():
    # 创建一个新的Toplevel窗口
    new_window = tk.Toplevel()
    new_window.title("New Window")
    # 速度控制子窗口
    AddUI_ComPWM(new_window)

'''
创建控件
'''
def createGUI():
    global win
    win = tk.Tk()
    displayWidth = win.winfo_screenwidth()  # 获取屏幕宽度
    displayHeight = win.winfo_screenheight()
    winWidth, winHeight = displayWidth, displayHeight - 70
    winX, winY = -8, 0
    # winX, winY = int((displayWidth - winWidth) /
    #                  2), int((displayHeight - winHeight - 70) / 2)
    win.title("窗口标题")
    win.geometry(
        '%dx%d-%d+%d' %
        (winWidth,
         winHeight,
         winX, winY))  # %dx%d宽度x 高度+横向偏移量(距左边)+纵向偏移量(距上边)
    # win.resizable(0, 0) # 不使能最大化
    win.protocol("WM_DELETE_WINDOW", closeWindow)
    # win.iconbitmap(r'resource/images/motor.ico')  # 窗口图标
    
    # AddUI_Com(win)

    graphFrame = tk.Frame(win) # 创建图表控件
    graphFrame.place(x=0, y=0)
    global tempGraphLabel
    tempGraphLabel = tempGraph(graphFrame)

    recv_data = threading.Thread(target=updataData)  # 开启线程
    recv_data.start()
    
    # 创建子窗口
    create_new_window()
    
    # AddUI_Com(win)
    # PID threading
    pid_control = threading.Thread(target=PID_control_motor_threading, args=(motor_target_speed,PID_motor_kpi,PID_motor_kdi,PID_motor_kdd))
    time.sleep(1)
    # pid_control.start()
    atcar.set_motors_pwm(motor_pwm)

    updateWindow()  # 更新画布
    win.mainloop()

# %% PID控制
# 定义PID参数
PID_motor_kpi=[0.5,0.25,0.25,0.25]  # 电机PID参数 Kpi
PID_motor_kdi=[0.01,0.005,0.005,0.005]  # 电机PID参数 Kdi
PID_motor_kdd=[1,1,1,1]  # 电机PID参数 Kdd

# 定义电机PID变量
motor_pid_output=[0,0,0,0]
motor_pid_error=[0,0,0,0]
motor_pid_last_error=[0,0,0,0]
motor_pid_integral=[0,0,0,0]

# 定义电机速度
motor_target_speed=[0,0,0,0]
motor_pwm=[1500,1500,1500,1500]

# PID Error 精度
PID_motor_error_precision = 5

# 定义PID控制函数
def PID_control_motor(target_speed,kpi,kdi,kdd):
    global motor_pwm
    global motor_current_speed
    global motor_target_speed
    global motor_pid_output
    global motor_pid_error
    global motor_pid_last_error
    global motor_direction
    target_speed=motor_target_speed
    # 计算PID输出
    for i in range(4):
        motor_pid_error[i] = target_speed[i] - motor_current_speed[i]
        # 若是误差在0.01以下，则不进行PID控制
        if abs(motor_pid_error[i]) < PID_motor_error_precision:
            motor_pid_error[i] = 0
        motor_pid_integral[i] += motor_pid_error[i]
        motor_pid_output[i] = kpi[i] * motor_pid_error[i] + \
                                            kdi[i] * motor_pid_integral[i] + \
                                            kdd[i] * (motor_pid_error[i] - motor_pid_last_error[i])
        motor_pid_last_error[i] = motor_pid_error[i]
        # 电机速度
        motor_pwm[i] += motor_pid_output[i]
        # 限幅
        if motor_pwm[i] > 2500:
            motor_pwm[i] = 2500
        elif motor_pwm[i] < 500:
            motor_pwm[i] = 500
        else:
            motor_pwm[i] = int(motor_pwm[i])
    # 设置电机速度
    atcar.set_motors_pwm(motor_pwm)
    if DEBUG_MODE:
        output_str = (
            f"Motor Current PWM: {motor_pwm}\n"
            f"Motor Current SPD: {motor_current_speed}\n"
            f"Motor Target SPD: {target_speed}\n"
            f"Motor PIDs Output: {motor_pid_output}\n"
            f"Motor PIDs Errors: {motor_pid_error}\n"
            f"Motor PIDs Integr: {motor_pid_integral}\n"
            f"Motor PIDs LError: {motor_pid_last_error}\n"
            ""
        )
        # print(output_str)
        update_motor_current_speed_label(output_str)
    
def PID_control_motor_threading(target_speed,kpi,kdi,kdd):
    while True:
        PID_control_motor(target_speed,kpi,kdi,kdd)
        time.sleep(0.1)


if __name__ == '__main__':
    # atcar.set_motor_speed(0, 1570)
    createGUI()
    